from CRC import * #import the Checksum functions
from RR_fiducials import *


import time
from BioloidHighLevelCmds import *

##################################################################################
#
# MAIN
#
##################################################################################
#


print "Init Comms"
initComm(8)

print "Init RoboRealm"
Initialize_Vision_API()
#filename = "datalog"+time.strftime("%d%b%Y%H%M%S", time.gmtime())+".txt"
#filelog = open(filename,"w")
#


print "Start Loop"
while(1):


    bioloidX, bioloidY, bioloidR, bioloidvalid = Get_Marker("bioloid")
    print "RoboRealm", bioloidX, bioloidY, bioloidR, bioloidvalid

    speed, orient = Get_Speed()
    print speed, orient
    sendCommand( int(speed), int(orient))

    time.sleep(0.5)
    imu = getIMUdata()
    print "IMU DATA ", imu

    #filelog.writelines( str(imu) )


#filelog.close()
